

public class DifferentialDriveKinematics {

	public double lwd;  // leftWheelDiameter
	public double rwd; // rightWheelDiameter
	public double d; // distanceBetweenWheels
	public double vr; //angularSpeedOfRightWheelInDegrees
	public double vl; // angularSpeedOfLeftWheel
	public double orientation;
	
	public double x, y, delta;
	
	public DifferentialDriveKinematics (double leftWheelDiameter,double rightWheelDiameter,double distanceBetweenWheels)
	{
		this.lwd = leftWheelDiameter;
		this.rwd = rightWheelDiameter;
		this.d = distanceBetweenWheels;		
		this.x = 0;
		this.y = 0;
		this.orientation = 0;
		this.delta = 0.1F; 
		
	}
	
	public void nextStep( double angularSpeedOfLeftWheel , double angularSpeedOfRightWheelInDegrees)
	{		
		double perimeter;
		perimeter = 2.0F * Math.PI * (d/2.0F);		
		vl = (angularSpeedOfLeftWheel / 360.0F) * perimeter;
		vr = (angularSpeedOfRightWheelInDegrees / 360.0F) * perimeter;		
		//orientation = getOrientation() + ( 0.5 * (vl - vr) * delta );			
		
		x = getX() + (0.5F * (vr + vl) * Math.cos(orientation)) * delta;
		y = getY() + (0.5F * (vr + vl) * Math.sin(orientation)) * delta;
		orientation = getOrientation() + ( 0.5F * (vl - vr) * delta );
	}
	
	public double getX() 	{		return x;	}	
	public double getY()	{		return y;	}		
	public double getOrientation(){ return orientation;   }	
}
